/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.KinectStick;
import edu.wpi.first.wpilibj.RobotDrive;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class KinectDrive extends IterativeRobot {
    
    RobotDrive drive;
    
    KinectStick leftArm;
    KinectStick rightArm;
    
    double leftArmValue;
    double rightArmValue;
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        
        drive           = new RobotDrive(1,2,3,4);
        
        leftArm         = new KinectStick(1);
        rightArm        = new KinectStick(2);
        
        leftArmValue    = 0;
        rightArmValue   = 0;
        
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        
        leftArmValue    = leftArm.getY();
        rightArmValue   = rightArm.getY();
        
        if (isInDeadZone(leftArmValue) && isInDeadZone(rightArmValue)) {
            drive.tankDrive(0, 0);
        } else if (!isInDeadZone(leftArmValue) && !isInDeadZone(rightArmValue)) {
            drive.tankDrive(leftArmValue, rightArmValue);
        } else if (!isInDeadZone(leftArmValue) && isInDeadZone(rightArmValue)) {
            drive.tankDrive(leftArmValue, 0);
        } else if (isInDeadZone(leftArmValue) && !isInDeadZone(rightArmValue)) {
            drive.tankDrive(0, rightArmValue);
        } else {
            drive.tankDrive(0, 0);
            System.out.println("Error");
        }
                
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        
    }
    
    public boolean isInDeadZone(double value) {
        if (value > -0.3 && value < 0.3) {
            return true;
        } else {
            return true;
        }
    }
    
}
